#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
import time

def move_drone():
    # 初始化ROS节点
    rospy.init_node('move_drone_node', anonymous=True)
    
    # 创建一个发布者，将消息发布到/cmd_vel_flu话题
    velocity_publisher = rospy.Publisher('/xtdrone/iris_0/cmd_vel_flu', Twist, queue_size=10)
    
    # 设置发布频率
    rate = rospy.Rate(10)  # 10 Hz
    
    # 创建Twist消息
    vel_msg = Twist()
    
    # 设置线速度为2，x方向
    vel_msg.linear.x = 2.0
    vel_msg.linear.y = 0.0
    vel_msg.linear.z = 0.0
    
    # 设置角速度为0
    vel_msg.angular.x = 0.0
    vel_msg.angular.y = 0.0
    vel_msg.angular.z = 0.0
    
    # 飞行时间 = 距离 / 速度
    duration = 8.0 / 2.0  # 8个单位，速度为2，飞行时间为4秒
    
    # 获取当前时间
    start_time = time.time()
    
    # 持续发布速度命令，直到飞行时间结束
    while time.time() - start_time < duration:
        velocity_publisher.publish(vel_msg)
        rate.sleep()
    
    # 停止飞行
    vel_msg.linear.x = 0.0
    velocity_publisher.publish(vel_msg)

if __name__ == '__main__':
    try:
        move_drone()
    except rospy.ROSInterruptException:
        pass
